#include "modbus.hpp"

// Initialize static members
std::map<std::string, std::shared_ptr<PortContext>> Modbus::s_port_contexts;
std::mutex Modbus::s_contexts_map_mutex;

Modbus::Modbus()
{
    // 启动异步线程
    m_async_thread = std::thread(&Modbus::async_thread_func, this);
}

Modbus::~Modbus()
{
    // 停止异步线程
    {
        std::lock_guard<std::mutex> lock(m_task_mutex);
        m_running = false;
    }
    m_task_cv.notify_one();
    if (m_async_thread.joinable()) {
        m_async_thread.join();
    }
    // m_port_ctx (shared_ptr) will automatically handle PortContext lifetime
    // and PortContext destructor will handle modbus_t cleanup.
}

void Modbus::async_thread_func()
{
    while (true) {
        std::function<void()> task;
        {
            std::unique_lock<std::mutex> lock(m_task_mutex);
            m_task_cv.wait(lock, [this] { return !m_running || !m_tasks.empty(); });

            if (!m_running && m_tasks.empty()) {
                return;
            }

            if (!m_tasks.empty()) {
                task = std::move(m_tasks.front());
                m_tasks.pop();
            }
        }
        if (task) {
            task();
        }
    }
}

bool Modbus::set_config_tcp(std::string ip_addr, uint32_t port, uint8_t unit_id, uint32_t timeout_s,
                            uint32_t timeout_ms)
{
    std::string context_key = "tcp:" + ip_addr + ":" + std::to_string(port);
    m_slave_id = unit_id;  // Store slave_id for this instance

    std::lock_guard<std::mutex> map_lock(s_contexts_map_mutex);

    auto it = s_port_contexts.find(context_key);
    if (it == s_port_contexts.end()) {
        // Context not found, create a new one
        modbus_t* new_ctx = modbus_new_tcp(ip_addr.c_str(), port);
        if (new_ctx == nullptr) {
            error_msg = "Failed to create modbus TCP context: " + std::string(modbus_strerror(errno));
            return false;
        }

        // Configure timeouts for the new context
        if (modbus_set_response_timeout(new_ctx, timeout_s, timeout_ms * 1000) == -1) {
            error_msg = "Failed to set response timeout for TCP context: " + std::string(modbus_strerror(errno));
            modbus_free(new_ctx);  // Clean up
            return false;
        }
        // Note: libmodbus documentation suggests modbus_set_byte_timeout is mainly for RTU.
        // For TCP, response timeout is usually sufficient. If byte timeout is desired for TCP:
        // if (modbus_set_byte_timeout(new_ctx, timeout_s, timeout_ms * 1000) == -1) {
        //     error_msg = "Failed to set byte timeout for TCP context: " + std::string(modbus_strerror(errno));
        //     modbus_free(new_ctx);
        //     return false;
        // }

        if (modbus_connect(new_ctx) == -1) {
            error_msg = "Failed to connect TCP: " + std::string(modbus_strerror(errno));
            modbus_free(new_ctx);  // Clean up
            return false;
        }

        // Creation and connection successful, create and store PortContext
        try {
            m_port_ctx = std::make_shared<PortContext>(context_key, new_ctx);
            s_port_contexts[context_key] = m_port_ctx;
        } catch (const std::bad_alloc& e) {
            error_msg = "Failed to allocate PortContext: " + std::string(e.what());
            modbus_close(new_ctx);
            modbus_free(new_ctx);
            return false;
        }
    } else {
        // Context found, share it
        m_port_ctx = it->second;
    }

    // The modbus_set_slave for the specific unit_id will be called before each read/write operation.
    return true;
}

bool Modbus::set_config_rtu(std::string port, uint32_t baud, uint8_t addr, uint32_t timeout_s, uint32_t timeout_ms)
{
    m_slave_id = addr;  // Store slave_id for this instance

    // Port name processing
    std::string processed_port_name;
#ifdef _WIN32
    if (port.find("COM") != std::string::npos && port.length() > 3 && std::isdigit(port[3])) {
        try {
            if (std::stoi(port.substr(3)) > 9) {
                processed_port_name = "\\\\.\\" + port;
            } else {
                processed_port_name = port;
            }
        } catch (const std::invalid_argument& ia) {
            // Handle cases like "COM" or "COMXYZ" that are not valid numbers after COM
            processed_port_name = port;
        } catch (const std::out_of_range& oor) {
            processed_port_name = port;
        }
    } else {
        processed_port_name = port;
    }
#else
    if (port.find("COM") != std::string::npos && port.length() > 3 && std::isdigit(port[3])) {
        try {
            int portNum = std::stoi(port.substr(3)) - 1;  // COM1 对应 ttyS0
            processed_port_name = "/dev/ttyS" + std::to_string(portNum);
        } catch (const std::invalid_argument& ia) {
            processed_port_name = port;
        } catch (const std::out_of_range& oor) {
            processed_port_name = port;
        }
    } else {
        processed_port_name = port;
    }
#endif

    std::string context_key = "rtu:" + processed_port_name;

    std::lock_guard<std::mutex> map_lock(s_contexts_map_mutex);

    auto it = s_port_contexts.find(context_key);
    if (it == s_port_contexts.end()) {
        // Context not found, create a new one
        modbus_t* new_ctx = modbus_new_rtu(processed_port_name.c_str(), baud, 'N', 8, 1);
        if (new_ctx == nullptr) {
            error_msg = "Failed to create modbus RTU context: " + std::string(modbus_strerror(errno));
            return false;
        }

        // Configure timeouts for the new context
        if (modbus_set_response_timeout(new_ctx, timeout_s, timeout_ms * 1000) == -1) {
            error_msg = "Failed to set response timeout for RTU context: " + std::string(modbus_strerror(errno));
            modbus_free(new_ctx);
            return false;
        }
        if (modbus_set_byte_timeout(new_ctx, timeout_s, timeout_ms * 1000) == -1) {  // Byte timeout is relevant for RTU
            error_msg = "Failed to set byte timeout for RTU context: " + std::string(modbus_strerror(errno));
            modbus_free(new_ctx);
            return false;
        }

        // libmodbus manual suggests setting slave ID on the context before connect for RTU,
        // but since we are sharing the context, we will set it before each operation.
        // However, some serial port configurations might require slave to be set before connect.
        // For now, we follow the dynamic slave ID approach.
        // If issues arise, modbus_set_slave(new_ctx, m_slave_id_for_initial_connect_if_any) could be considered here.

        if (modbus_connect(new_ctx) == -1) {
            error_msg = "Failed to connect RTU (" + processed_port_name + "): " + std::string(modbus_strerror(errno));
            modbus_free(new_ctx);
            return false;
        }

        // Creation and connection successful, create and store PortContext
        try {
            m_port_ctx = std::make_shared<PortContext>(context_key, new_ctx);
            s_port_contexts[context_key] = m_port_ctx;
        } catch (const std::bad_alloc& e) {
            error_msg = "Failed to allocate PortContext: " + std::string(e.what());
            modbus_close(new_ctx);
            modbus_free(new_ctx);
            return false;
        }
    } else {
        // Context found, share it
        m_port_ctx = it->second;
    }
    // The modbus_set_slave for the specific slave_addr will be called before each read/write operation.
    return true;
}

bool Modbus::read_data(uint16_t address, uint16_t length, std::vector<uint16_t>& data)
{
    if (!m_port_ctx || !m_port_ctx->ctx) {
        error_msg = "Modbus context not configured or invalid.";
        return false;
    }

    std::lock_guard<std::mutex> op_lock(m_port_ctx->operation_mutex);

    if (modbus_set_slave(m_port_ctx->ctx, m_slave_id) == -1) {
        error_msg = "Failed to set slave ID (" + std::to_string(m_slave_id) +
                    ") for read: " + std::string(modbus_strerror(errno));
        return false;
    }

    data.resize(length);
    if (modbus_read_registers(m_port_ctx->ctx, address, length, data.data()) == -1) {
        error_msg = "Failed to read registers: " + std::string(modbus_strerror(errno));
        return false;
    }
    return true;
}

bool Modbus::read_data(uint16_t address, uint16_t length, std::vector<float>& data, MB_DataOrder order)
{
    // 每个 float 需要两个 uint16_t
    std::vector<uint16_t> temp(length * 2);
    if (!read_data(address, temp.size(), temp)) {
        return false;
    }
    data.resize(length);
    // 转换字节序
    for (size_t i = 0; i < length; ++i) {
        uint32_t combined;
        size_t idx = i * 2;  // temp数组的索引

        switch (order) {
            case MB_ABCD:  // 大端序 (1234)
                combined = (static_cast<uint32_t>(temp[idx]) << 16) | temp[idx + 1];
                break;

            case MB_BADC:  // 字交换 (2143)
                combined = (static_cast<uint32_t>(temp[idx]) & 0xFF00) << 8 |
                           (static_cast<uint32_t>(temp[idx]) & 0x00FF) << 24 | (temp[idx + 1] & 0xFF00) >> 8 |
                           (temp[idx + 1] & 0x00FF) << 8;
                break;

            case MB_CDAB:  // 字节交换 (3412)
                combined = (static_cast<uint32_t>(temp[idx + 1]) << 16) | temp[idx];
                break;

            case MB_DCBA:  // 小端序 (4321)
                combined = (static_cast<uint32_t>(temp[idx]) & 0xFF00) >> 8 |
                           (static_cast<uint32_t>(temp[idx]) & 0x00FF) << 8 | (temp[idx + 1] & 0xFF00) >> 24 |
                           (temp[idx + 1] & 0x00FF) >> 8;
                break;

            default:
                error_msg = "Invalid byte order";
                return false;
        }

        union {
            uint32_t i;
            float f;
        } u;
        u.i = combined;
        data[i] = u.f;
    }
    return true;
}

bool Modbus::write_data(uint16_t address, uint16_t length, const std::vector<uint16_t>& data)
{
    if (!m_port_ctx || !m_port_ctx->ctx) {
        error_msg = "Modbus context not configured or invalid.";
        return false;
    }
    if (length == 0 && data.empty()) return true;  // Nothing to write
    if (data.size() < length) {
        error_msg = "Insufficient data provided for write operation.";
        return false;
    }

    std::lock_guard<std::mutex> op_lock(m_port_ctx->operation_mutex);

    if (modbus_set_slave(m_port_ctx->ctx, m_slave_id) == -1) {
        error_msg = "Failed to set slave ID (" + std::to_string(m_slave_id) +
                    ") for write: " + std::string(modbus_strerror(errno));
        return false;
    }

    if (modbus_write_registers(m_port_ctx->ctx, address, length, data.data()) == -1) {
        error_msg = "Failed to write registers: " + std::string(modbus_strerror(errno));
        return false;
    }
    return true;
}

bool Modbus::write_data(uint16_t address, uint16_t length, const std::vector<float>& data, MB_DataOrder order)
{
    if (length == 0 && data.empty()) return true;  // Nothing to write
    if (data.size() < length) {
        error_msg = "Insufficient float data provided for write operation.";
        return false;
    }
    // 每个 float 需要两个 uint16_t
    std::vector<uint16_t> temp(length * 2);

    // 转换字节序
    for (size_t i = 0; i < length; ++i) {
        union {
            float f;
            uint32_t i;
        } u;
        u.f = data[i];
        uint32_t value = u.i;
        size_t idx = i * 2;  // temp数组的索引

        switch (order) {
            case MB_ABCD:  // 大端序 (1234)
                temp[idx] = (value >> 16) & 0xFFFF;
                temp[idx + 1] = value & 0xFFFF;
                break;

            case MB_BADC:  // 字交换 (2143)
                temp[idx] = ((value >> 8) & 0xFF00) | ((value >> 24) & 0x00FF);
                temp[idx + 1] = ((value << 8) & 0xFF00) | ((value >> 8) & 0x00FF);
                break;

            case MB_CDAB:  // 字节交换 (3412)
                temp[idx] = value & 0xFFFF;
                temp[idx + 1] = (value >> 16) & 0xFFFF;
                break;

            case MB_DCBA:  // 小端序 (4321)
                temp[idx] = ((value << 8) & 0xFF00) | ((value >> 8) & 0x00FF);
                temp[idx + 1] = ((value << 24) & 0xFF00) | ((value << 8) & 0x00FF);
                break;

            default:
                error_msg = "Invalid byte order";
                return false;
        }
    }

    return write_data(address, temp.size(), temp);
}

void Modbus::async_read_data(uint16_t address, uint16_t length,
                             std::function<void(bool, std::vector<uint16_t>&)> callback)
{
    std::lock_guard<std::mutex> lock(m_task_mutex);
    m_tasks.push([this, address, length, callback]() {
        std::vector<uint16_t> data(length);
        bool success = read_data(address, length, data);
        if (callback) {
            callback(success, data);
        }
    });
    m_task_cv.notify_one();
}

void Modbus::async_read_data(uint16_t address, uint16_t length, std::function<void(bool, std::vector<float>&)> callback,
                             MB_DataOrder order)
{
    std::lock_guard<std::mutex> lock(m_task_mutex);
    m_tasks.push([this, address, length, callback, order]() {
        std::vector<float> data(length);
        bool success = read_data(address, length, data, order);
        if (callback) {
            callback(success, data);
        }
    });
    m_task_cv.notify_one();
}

void Modbus::async_write_data(uint16_t address, uint16_t length, const std::vector<uint16_t>& data,
                              std::function<void(bool)> callback)
{
    std::lock_guard<std::mutex> lock(m_task_mutex);
    m_tasks.push([this, address, length, data, callback]() {
        bool success = write_data(address, length, data);
        if (callback) {
            callback(success);
        }
    });
    m_task_cv.notify_one();
}

void Modbus::async_write_data(uint16_t address, uint16_t length, const std::vector<float>& data,
                              std::function<void(bool)> callback, MB_DataOrder order)
{
    std::lock_guard<std::mutex> lock(m_task_mutex);
    m_tasks.push([this, address, length, data, callback, order]() {
        bool success = write_data(address, length, data, order);
        if (callback) {
            callback(success);
        }
    });
    m_task_cv.notify_one();
}